#include "gimbal_uart.h"
#include <stdio.h>
using namespace std;

GimbalUart::GimbalUart():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,57600);
    _to = Timeout::simpleTimeout(100);

    _gimbal_vel_sub = nh.subscribe("staple_to_gimbal",100,&GimbalUart::GimbalVelCallback,this);

}

void GimbalUart::GimbalVelCallback(const unionsys_msgs::Gimbal_PidVel_cmd& msg)
{
    int v_yaw = msg.v_yaw;
    int v_pitch = msg.v_pitch;

    char* vel_yaw = new char(8);
    char* vel_pitch = new char(8);
    char* res = new char(8);
    char* res_pitch = new char(8);
    sprintf(vel_yaw,"%X",v_yaw);
    sprintf(vel_pitch,"%X",v_pitch);
    if(v_yaw>0)
    {
        if(v_yaw<16)
        {
            res[0] = '0';
            res[1] = vel_yaw[0];
        }
        else
        {
            res[0] = vel_yaw[0];
            res[1] = vel_yaw[1];
        }
    }
    else
    {
        if(-v_yaw<16)
        {
            res[0] = '0';
            res[1] = vel_yaw[7];
        }
        else
        {
            res[0] = vel_yaw[6];
            res[1] = vel_yaw[7];
        }
    }


    if(v_pitch>0)
    {
        if(v_pitch<16)
        {
            res_pitch[0] = '0';
            res_pitch[1] = vel_pitch[0];
        }
        else
        {
            res_pitch[0] = vel_pitch[0];
            res_pitch[1] = vel_pitch[1];
        }
    }
    else
    {
        if(-v_pitch<16)
        {
            res_pitch[0] = '0';
            res_pitch[1] = vel_pitch[7];
        }
        else
        {
            res_pitch[0] = vel_pitch[6];
            res_pitch[1] = vel_pitch[7];
        }
    }

    char temp[16] = {'#','t','p','U','G','4','w','G','S','M',res[0],res[1],res_pitch[0],res_pitch[1]};
    char* package = StringToHex_s(temp,14);
    char send_package[16]={0};

    for(uint8_t i=0;i<14;i++)
    {
        char left = package[i*2];
        char right = package[i*2+1];
        std::cout<<left<<right<<std::endl;
        std::cout<<(int)left<<(int)right<<std::endl;
        switch (left) {
        case 'A':
            left = 10;
            break;
        case 'B':
            left = 11;
            break;
        case 'C':
            left = 12;
            break;
        case 'D':
            left = 13;
            break;
        case 'E':
            left = 14;
            break;
        case 'F':
            left = 15;
            break;
        default:
            left = left-'0';
            break;
        }


        switch (right) {
        case 'A':
            right = 10;
            break;
        case 'B':
            right = 11;
            break;
        case 'C':
            right = 12;
            break;
        case 'D':
            right = 13;
            break;
        case 'E':
            right = 14;
            break;
        case 'F':
            right = 15;
            break;
        default:
            right = right-'0';
            break;
        }

        send_package[i] = left*16+right;
    }

    char crc = CrcCheckForGimbal(send_package,14);
    char tmp[3];
    sprintf(tmp,"%02X",crc);

    temp[14] = tmp[0];
    temp[15] = tmp[1];

    char* buffer = StringToHex_s(temp,16);
    for(uint8_t i=0;i<16;i++)
    {
        char left = buffer[i*2];
        char right = buffer[i*2+1];
        switch (left) {
        case 'A':
            left = 10;
            break;
        case 'B':
            left = 11;
            break;
        case 'C':
            left = 12;
            break;
        case 'D':
            left = 13;
            break;
        case 'E':
            left = 14;
            break;
        case 'F':
            left = 15;
            break;
        default:
            left = left-'0';
            break;
        }


        switch (right) {
        case 'A':
            right = 10;
            break;
        case 'B':
            right = 11;
            break;
        case 'C':
            right = 12;
            break;
        case 'D':
            right = 13;
            break;
        case 'E':
            right = 14;
            break;
        case 'F':
            right = 15;
            break;
        default:
            right = right-'0';
            break;
        }

        send_package[i] = left*16+right;

    }

    write(send_package);
}


string GimbalUart::toHex(int num) {
    const string HEX = "0123456789ABCDEF";
    if (num == 0) return "0";
    string result;
    int count = 0;
    while (num && count++ < 8) {
        result = HEX[(num & 0xf)] + result;
        num >>= 4;
    }
    return result;
}



char *GimbalUart::StringToHex_s(char *String, int Inlen)
{
    if (String == NULL || String == nullptr) { return nullptr; }

    int BufLen = Inlen * 2 + 1;

    char *Buf = new char[BufLen];
    memset(Buf, 0, BufLen);

    char buf[3] = { 0 };

    for (int i = 0; i < Inlen; i++)
    {

        sprintf(buf, "%02X", (unsigned char)String[i]);
        (Buf + (i * 2))[0] = buf[0];
        (Buf + (i * 2))[1] = buf[1];


    }
    return Buf;
}


void GimbalUart::test()
{
    char temp[16] = {'#','t','p','U','G','4','w','G','S','M','E','2','E','2'};//res[0],res[1],res_pitch[0],res_pitch[1]};
    char* package = StringToHex_s(temp,14);
    char send_package[16]={0};

    for(uint8_t i=0;i<14;i++)
    {
        char left = package[i*2];
        char right = package[i*2+1];
        std::cout<<left<<right<<std::endl;
        std::cout<<(int)left<<(int)right<<std::endl;
        switch (left) {
        case 'A':
            left = 10;
            break;
        case 'B':
            left = 11;
            break;
        case 'C':
            left = 12;
            break;
        case 'D':
            left = 13;
            break;
        case 'E':
            left = 14;
            break;
        case 'F':
            left = 15;
            break;
        default:
            left = left-'0';
            break;
        }


        switch (right) {
        case 'A':
            right = 10;
            break;
        case 'B':
            right = 11;
            break;
        case 'C':
            right = 12;
            break;
        case 'D':
            right = 13;
            break;
        case 'E':
            right = 14;
            break;
        case 'F':
            right = 15;
            break;
        default:
            right = right-'0';
            break;
        }

        send_package[i] = left*16+right;
    }

    char crc = CrcCheckForGimbal(send_package,14);
    char tmp[2];
    sprintf(tmp,"%02X",crc);

    temp[14] = tmp[0];
    temp[15] = tmp[1];

    char* buffer = StringToHex_s(temp,16);
    for(uint8_t i=0;i<16;i++)
    {
        char left = buffer[i*2];
        char right = buffer[i*2+1];
        switch (left) {
        case 'A':
            left = 10;
            break;
        case 'B':
            left = 11;
            break;
        case 'C':
            left = 12;
            break;
        case 'D':
            left = 13;
            break;
        case 'E':
            left = 14;
            break;
        case 'F':
            left = 15;
            break;
        default:
            left = left-'0';
            break;
        }


        switch (right) {
        case 'A':
            right = 10;
            break;
        case 'B':
            right = 11;
            break;
        case 'C':
            right = 12;
            break;
        case 'D':
            right = 13;
            break;
        case 'E':
            right = 14;
            break;
        case 'F':
            right = 15;
            break;
        default:
            right = right-'0';
            break;
        }

        send_package[i] = left*16+right;

    }
}

void GimbalUart::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t GimbalUart::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}

char GimbalUart::CrcCheckForGimbal(char* ch,int len)
{
    char crc = 0;
    for(uint8_t i=0;i<len;i++)
    {
        crc += ch[i];
    }

    return crc;
}

void GimbalUart::RunMain()
{

    test();
    OpenSerial();
    ros::Rate loop_rate(10);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
}
